/*
Class Helmholtz implements a period-length detector using Philip McLeod's
Specially Normalized AutoCorrelation function (SNAC).

Licensed under three-clause BSD license.
 
Katja Vetter, Feb 2012.
 
*/


#include <stdlib.h>
#include <math.h>
#include "Helmholtz.h"


DllExport Helmholtz::Helmholtz(int framearg, int overlaparg, t_float biasarg) : Processor()
{
    inputbuf = NULL;
    inputbuf2 = NULL;
    processbuf = NULL;
    
    setframesize(framearg);
    setoverlap(overlaparg);
    if(biasarg)setbias(biasarg);
    else biasfactor = DEFBIAS;
    
    inputbuf = (t_float*)calloc(framesize, sizeof(t_float));
    inputbuf2 = (t_float*)calloc(framesize, sizeof(t_float));
    processbuf = (t_float*)calloc(framesize * 2, sizeof(t_float));
        
    timeindex = 0;
    periodindex = 0;
    periodlength = 0.;
    fidelity = 0.;
    minrms = DEFMINRMS;


	pitchGraph = NULL;
	sinwavePitchGraph = NULL;
}


Helmholtz::~Helmholtz()
{
	if (inputbuf!= NULL)
		free(inputbuf);

    inputbuf = NULL;

	if (inputbuf2 != NULL)
		free(inputbuf2);

    inputbuf2 = NULL;

	if (processbuf != NULL)
		free(processbuf);

    processbuf = NULL;

	if (pitchGraph){
		delete [] pitchGraph;
		pitchGraph = NULL;
	}

	if (sinwavePitchGraph){
		delete[]sinwavePitchGraph;
		sinwavePitchGraph = NULL;
	}
}
  
/*********************************************************************************/
/******************************** public *****************************************/
/*********************************************************************************/          


void Helmholtz::iosamples(t_float *in, t_float *out, int size)
{
    int mask = framesize - 1;
    int outindex = 0;
    
    // call analysis function when it is time
    if(!(timeindex & (framesize / overlap - 1))) analyzeframe();
    
    while(size--) 
    {
        inputbuf[timeindex] = *in++;
        out[outindex++] = processbuf[timeindex++];
        timeindex &= mask;
    }
}    


void Helmholtz::setframesize(int frame)
{
	if (!((frame == 128) | (frame == 256) | (frame == 512) | (frame == 1024) | (frame == 2048) | (frame == 4096)))
        frame = DEFFRAMESIZE;
    framesize = frame;
    
    if(inputbuf) inputbuf = (t_float*)realloc(inputbuf, framesize * sizeof(t_float));
    if(inputbuf2) inputbuf2 = (t_float*)realloc(inputbuf2, framesize * sizeof(t_float));
    if(processbuf) processbuf = (t_float*)realloc(processbuf, framesize * 2 * sizeof(t_float));
    
    timeindex = 0;
}


void Helmholtz::setoverlap(int lap)
{
	if (!((lap == 1) | (lap == 2) | (lap == 4) | (lap == 8) | (lap == 16) | (lap == 32) | (lap == 64)))
        lap = DEFOVERLAP;
    overlap = lap;
}


void Helmholtz::setbias(t_float bias)
{
    if(bias > 1.) bias = 1.;
    if(bias < 0.) bias = 0.;
    biasfactor = bias;
}


void Helmholtz::setminRMS(t_float rms)
{
    if(rms > 1.) rms = 1.;
    if(rms < 0.) rms = 0.;
    minrms = rms;
}
    

t_float Helmholtz::getperiod() const
{
    return(periodlength);
}


t_float Helmholtz::getfidelity() const
{
    return(fidelity);
}


/*********************************************************************************/
/***************************** private procedures ********************************/
/*********************************************************************************/


// main analysis function
void Helmholtz::analyzeframe()
{
    int n, tindex = timeindex;
    int mask = framesize - 1;
    t_float norm = 1. / sqrt(t_float(framesize * 2));
    
    // copy input to processing buffer
    for(n=0; n<framesize; n++) processbuf[n] = inputbuf[tindex++ & mask] * norm;
    
    // copy for normalization function
    for(n=0; n<framesize; n++) inputbuf2[n] = inputbuf[tindex++ & mask];
    
    // zeropadding
    for(n=framesize; n<(framesize<<1); n++) processbuf[n] = 0.;
    
    // call analysis procedures
    autocorrelation();
    normalize();
    pickpeak();
    periodandfidelity();
}


void Helmholtz::autocorrelation()
{
    int n;
    int fftsize = framesize * 2;
    
    REALFFT(fftsize, processbuf);
    
    // compute power spectrum
    processbuf[0] *= processbuf[0]; // DC
    processbuf[framesize] *= processbuf[framesize]; // Nyquist
    
    for(n=1; n<framesize; n++)                                  
    {
        processbuf[n] = processbuf[n] * processbuf[n]
           + processbuf[fftsize-n] * processbuf[fftsize-n]; // imag coefficients appear reversed
        processbuf[fftsize-n] = 0.;
        
    }
    
    REALIFFT(fftsize, processbuf);
}


void Helmholtz::normalize()
{
    int n, mask = framesize - 1;
    int seek = framesize * SEEK;
    t_float signal1, signal2;
    
    // minimum RMS implemented as minimum autocorrelation at index 0
    // effectively this means possible white noise addition
    t_float rms = minrms / sqrt(1. / (t_float)framesize);
    t_float minrzero = rms * rms;
    t_float rzero = processbuf[0];
    if(rzero < minrzero) rzero = minrzero;
    double normintegral = rzero * 2.;
    
    // normalize biased autocorrelation function
    processbuf[0] = 1.;
    for(n=1; n<seek; n++)
    {
       signal1 = inputbuf2[n-1];
       signal2 = inputbuf2[framesize-n];
       normintegral -= (double)(signal1 * signal1 + signal2 * signal2);
       processbuf[n] /= (t_float)normintegral * 0.5;
    }
    
    // flush instable function tail
    for(n = seek; n<framesize; n++) processbuf[n] = 0.;
}
    

// select the peak which most probably represents period length 
void Helmholtz::pickpeak()
{
    int n, peakindex=0;
    int seek = framesize * SEEK;
    t_float maxvalue = 0.;
    t_float bias = biasfactor / (t_float)framesize;    // user-controlled bias
    t_float realpeak;
    
    // skip main lobe
    for(n=1; n<seek; n++)
    {
        if(processbuf[n] < 0.) break;
    }
    
    // find interpolated / biased maximum in specially normalized autocorrelation function
    // interpolation finds the 'real maximum'
    // biasing favours the first candidate
    for(; n<seek-1; n++)
    {
        if(processbuf[n] > processbuf[n-1]) 
        {
            if(processbuf[n] > processbuf[n+1]) // we have a local peak
            {
                realpeak = interpolate3max(processbuf, n);
                
                if((realpeak * (1. - n * bias)) > maxvalue) 
                {
                    maxvalue = realpeak;
                    peakindex = n;
                }
            }
        }
    }
    periodindex = peakindex;
}    


void Helmholtz::periodandfidelity()
{
    if(periodindex)
    {
        periodlength = periodindex + interpolate3phase(processbuf, periodindex);
        fidelity = interpolate3max(processbuf, periodindex);
    }
}

    
/*********************************************************************************/    
/***************************** private functions *********************************/    
/*********************************************************************************/


inline t_float Helmholtz::interpolate3max(t_float *buf, int peakindex)
{
    t_float realpeak;
    
    t_float a = buf[peakindex-1];
    t_float b = buf[peakindex];
    t_float c = buf[peakindex+1];
    
    realpeak = b + 0.5 * (0.5 * ((c - a) * (c - a))) 
                / (2 * b - a - c);
    
    return(realpeak);
    
}

inline t_float Helmholtz::interpolate3phase(t_float *buf, int peakindex)
{
    t_float fraction;
    
    t_float a = buf[peakindex-1];
    t_float b = buf[peakindex];
    t_float c = buf[peakindex+1];
    
    fraction = (0.5 * (c - a)) / ( 2. * b - a - c);
    
    return(fraction);
}

/////////////////////////////////////////////////////////////////////////////
// Processing

void  Helmholtz::processingBlock(DATA_TYPE * startFrame, LENGTH_TYPE frameSize){

	this->iosamples(startFrame, hhout, frameSize);

	DATA_TYPE initialFrequency = ((OptionHelmholtz *)dataChannel)->initFrequency;
	DATA_TYPE freq_Hel = initialFrequency;
	DATA_TYPE harmonic;
	//Helmoholtz
	if (this->getperiod() > 0.f){
		freq_Hel = DATA_TYPE(dataChannel->SampleRate) / this->getperiod();

		// compensate for harmonic jumping //BYPASS THIS IF NO FREQUENCY SPECIFIED
		if (initialFrequency > 0){
			harmonic = freq_Hel / initialFrequency;
			if (harmonic < 1)
				freq_Hel = freq_Hel * round(1 / harmonic);
			else
				freq_Hel = freq_Hel / round(harmonic);

			if (this->getfidelity() >= option.Fidelity)
				hold_freq = freq_Hel;
			else
				freq_Hel = hold_freq;
		}
		else{
			hold_freq = freq_Hel;
		}
	}

	//calculate average frequency
	// Lay pitch data
		if (this->getfidelity() > .95){
			if (((OptionHelmholtz *)dataChannel)->Avg_Name){
				pitchs.push_back(freq_Hel);
				pitchs_rounded.push_back((int)round(freq_Hel));
			}
		}

	// calculate the phase increment for the detected frequency
	// Lay sin wave grap va graph wave.
		if (((OptionHelmholtz *)dataChannel)->SineWave_Graph || ((OptionHelmholtz *)dataChannel)->Pitch_Graph){
		
		double phaseIncr_Hel = 0.0;
		double phaseLine_Hel = 0.0;
		phaseIncr_Hel = ((1.f / dataChannel->SampleRate) * freq_Hel) * TWOPI;
		if (freq_Hel > 0){}
		else{
			phaseLine_Hel = ((1.f / dataChannel->SampleRate) * hold_freq) * TWOPI*(frameSize - 1);
		}
		double newPitch_Hel = 0;
		if (initialFrequency > 0)
			newPitch_Hel = initialFrequency / freq_Hel - 1;
		else
			newPitch_Hel = 1.0 / freq_Hel; //NO longer need this now


		double phaseIncr_Sig = 0.0;
		double phaseLine_Sig = 0.0;

		phaseLine_Hel = ((1.f / dataChannel->SampleRate) * hold_freq) * TWOPI*(frameSize - 1);


		for (LENGTH_TYPE s = 0; s<frameSize; s++)
		{
			// Lay Sin wave Grap data
			if (((OptionHelmholtz *)dataChannel)->SineWave_Graph) {
				
				if (freq_Hel > 0){
					sinwavePitchGraph[pos] = sinf(phase_Hel);
				}
				else {
					sinwavePitchGraph[pos] = sinf(phaseLine_Hel);
				}

			}

			//Lay Pitch Data
			if (((OptionHelmholtz *)dataChannel)->Pitch_Graph) {
				pitchGraph[pos] = newPitch_Hel;
			}

			pos++;
			phase_Hel += phaseIncr_Hel;

		}
	}
}

void Helmholtz::processing(){
	
	if (!hhout)
		hhout = new float[option.BlockSize];
	else{
		delete[] hhout;
		hhout = new float[option.BlockSize];
	}

	if (dataChannel){
		if (((OptionHelmholtz *)dataChannel)->Pitch_Graph){
			if (!sinwavePitchGraph)
				pitchGraph = new DATA_TYPE[dataChannel->Length];
			else{
				delete[] pitchGraph;
				pitchGraph = new DATA_TYPE[dataChannel->Length];
			}
		}
		if (((OptionHelmholtz *)dataChannel)->SineWave_Graph){
			if (!sinwavePitchGraph)
				sinwavePitchGraph = new DATA_TYPE[dataChannel->Length];
			else{
				delete[] sinwavePitchGraph;
				sinwavePitchGraph = new DATA_TYPE[dataChannel->Length];
			}
		}

		// Xy ly tinh pitchs
		Processor::processing();
		// Tinh trung binh pitch
		avgPitch();
	}
	
	
}

DATA_TYPE *Helmholtz::getPitchGraph(){
	return pitchGraph;
};

DATA_TYPE *Helmholtz::getSinwavePitchGraph(){
	return sinwavePitchGraph;
};

void Helmholtz::avgPitch(){
	int most = 0;
	DATA_TYPE initialFrequency = ((OptionHelmholtz *)dataChannel)->initFrequency;
	if (initialFrequency != 0)
		most = initialFrequency;
	else
		most = pitch_appear_the_most();
	float first = most *RANGE_LO;
	float seconds = most *RANGE_HI;
	float sum = 0;
	int times = 0;
	for (unsigned int i = 0; i < pitchs.size(); i++){
		if (pitchs[i] < seconds && pitchs[i] > first){
			times++;
			sum += pitchs[i];
		}
	}
	avg_pitchs =  sum / times;
}

float Helmholtz::getAvgPitch(){
	return avg_pitchs;
}

int Helmholtz::pitch_appear_the_most(){
	int hold = 0;
	unsigned int i = 0;
	int max = 0;
	while (i < pitchs_rounded.size() - 1){
		int count = 0;
		while (pitchs_rounded[i] == pitchs_rounded[i + 1]){
			i++;
			count++;
		}
		if (max < count){
			max = count;
			hold = pitchs_rounded[i - 1];
		}
		i++;
	}
	return hold;
}

int Helmholtz::Save(audio::AudioFileOut &out){

	if (((OptionHelmholtz *)dataChannel)->Avg_Name){
		out.setAvgFrequency(this->getAvgPitch());
	}

	if (((OptionHelmholtz *)dataChannel)->Pitch_Graph){
		out.AddChannel(this->getPitchGraph());
	}
		
	if (((OptionHelmholtz *)dataChannel)->SineWave_Graph){
		out.AddChannel(this->getSinwavePitchGraph());
	}
	return 0;
}